# Car's movement control (forward, back, left, right, brake)
# motor control
import math
import RPi.GPIO as GPIO
import time

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)


class CarMove(object):
    def __init__(self):
        GPIO_motor_1 = 18  # GPIO 设置，与板子上的引脚对应
        GPIO_motor_4 = 23
        GPIO_motor_5 = 25
        GPIO_motor_6 = 24

        self.GPIO_supervoice_in=20   #超声波数据入
        self.GPIO_supervoice_out=21  #超声波控制出

        GPIO.setup(GPIO_motor_1, GPIO.OUT)  # 设置该引脚为输出
        GPIO.setup(GPIO_motor_4, GPIO.OUT)
        GPIO.setup(GPIO_motor_5, GPIO.OUT)
        GPIO.setup(GPIO_motor_6, GPIO.OUT)
        GPIO.setup(self.GPIO_supervoice_in,GPIO.IN)  #设置引脚模式
        GPIO.setup(self.GPIO_supervoice_out,GPIO.OUT)

        self.motor_1 = GPIO.PWM(GPIO_motor_1, 500)  #通道，频率，创建PWM实例
        self.motor_4 = GPIO.PWM(GPIO_motor_4, 500)
        self.motor_5 = GPIO.PWM(GPIO_motor_5, 500)
        self.motor_6 = GPIO.PWM(GPIO_motor_6, 500)

        self.motor_1.start(0)  # #启动PWM dc值（0.0<dc<100.0）
        self.motor_4.start(0)
        self.motor_5.start(0)
        self.motor_6.start(0)

    def back(self, speed):
        self.motor_1.ChangeDutyCycle(speed)  # 占空比0.0-100.0
        self.motor_4.ChangeDutyCycle(0)
        self.motor_5.ChangeDutyCycle(speed)
        self.motor_6.ChangeDutyCycle(0)

    def forward(self, speed):
        self.motor_1.ChangeDutyCycle(0)
        self.motor_4.ChangeDutyCycle(speed)
        self.motor_5.ChangeDutyCycle(0)
        self.motor_6.ChangeDutyCycle(speed)

    def right(self, speed):
        self.motor_1.ChangeDutyCycle(0)
        self.motor_4.ChangeDutyCycle(0)
        self.motor_5.ChangeDutyCycle(0)
        self.motor_6.ChangeDutyCycle(speed)

    def left(self, speed):
        self.motor_1.ChangeDutyCycle(0)
        self.motor_4.ChangeDutyCycle(speed)
        self.motor_5.ChangeDutyCycle(0)
        self.motor_6.ChangeDutyCycle(0)

    def brake(self):
        self.motor_1.ChangeDutyCycle(0)
        self.motor_4.ChangeDutyCycle(0)
        self.motor_5.ChangeDutyCycle(0)
        self.motor_6.ChangeDutyCycle(0)

    def forward_turn(self, speed_left, speed_right):
        if speed_left>100:
            speed_left=100
        if speed_right>100:
            speed_right=100
        if speed_left<0:
            speed_left=0
        if speed_right<0:
            speed_right=0
        self.motor_1.ChangeDutyCycle(0)
        self.motor_4.ChangeDutyCycle(speed_left)
        self.motor_5.ChangeDutyCycle(0)
        self.motor_6.ChangeDutyCycle(speed_right)
        # print(speed_left,speed_right)

    def back_turn(self, speed_left, speed_right):
        if speed_left>100:
            speed_left=100
        if speed_right>100:
            speed_right=100
        if speed_left<0:
            speed_left=0
        if speed_right<0:
            speed_right=0
        self.motor_1.ChangeDutyCycle(speed_left)
        self.motor_4.ChangeDutyCycle(0)
        self.motor_5.ChangeDutyCycle(speed_right)
        self.motor_6.ChangeDutyCycle(0)

    def MotorStop(self):
        self.motor_1.stop()
        self.motor_4.stop()
        self.motor_5.stop()
        self.motor_6.stop()

    def track_move_left(self, speed, diff):
        self.motor_1.ChangeDutyCycle(40)
        self.motor_4.ChangeDutyCycle(20)
        self.motor_5.ChangeDutyCycle(20)
        self.motor_6.ChangeDutyCycle(40)

    def get_dis(self):
        GPIO.output(self.GPIO_supervoice_out,GPIO.HIGH)
        time.sleep(0.000015)
        GPIO.output(self.GPIO_supervoice_out,GPIO.LOW)
        time_flag=time.time()
        while not GPIO.input(self.GPIO_supervoice_in):
            pass
        timestart = time.time()
        while GPIO.input(self.GPIO_supervoice_in):
            pass
        return (time.time()-timestart) * 340 /2

def control(speed,theta,car):
    speed_change=speed-5
    if theta==-180:
        theta=0
        speed=-30
    else:
        if theta > 90:
            theta=90
        if theta <-90:
            theta=-90
    #car = CarMove()
    if speed>0:
        if theta<0:
            car.forward_turn(speed+speed_change*theta/90-5,speed-speed_change*theta/90+18)
        elif theta>0:
            car.forward_turn(speed+speed_change*theta/90+4,speed-speed_change*theta/90)
        else:
            car.forward_turn(speed,speed+10)

    if speed<0:
        speed=-1*speed
        car.back_turn(speed-speed_change*theta/90,speed+speed_change*theta/90)

    if speed==0:
        car.brake()

def stop(car):
    car.brake()
    #GPIO.cleanup()

if __name__=='__main__':
    car = CarMove()
    control(45,0,car)
    time.sleep(0.2)
    control(45,-60,car)
    time.sleep(1)
